 # @file   K210color.py
# @brief  2024中国机器人大赛总决赛 探险组 K210视觉识别模块
# @note   交通指示牌颜色判断
#         黑色 CMYK C:000;M:000;Y:000;K:100
#         蓝色 CMYK C:100;M:000;Y:100;K:000
#         绿色 CMYK C:000;M:100;Y:000;K:000
#         判断到黑色 K210板载红色LED亮起 排针IO6输出1
#         判断到蓝色 K210板载蓝色LED亮起 排针IO7输出1
#         判断到绿色 K210板载绿色LED亮起 排针IO8输出1
#         使能EN EN==1 K210向STM32发送数据 反之不发送
# @author sgfz
# @date   2024-09

# 导入库
import utime, time, sensor, image,lcd
# 导入IO映射管理库
from fpioa_manager import fm
# 导入K210 板载RGB GPIO模块
from Maix import GPIO

#################### 颜色指示与信号输出设置 ####################

# K210的RGB LED引脚
# IO 12 LED_G
# IO 13 LED_R
# IO 14 LED_B

IO_LED_G = 12
IO_LED_R = 13
IO_LED_B = 14

# 注册板载LED引脚到GPIO资源
fm.register(IO_LED_G, fm.fpioa.GPIO0, force = True)
fm.register(IO_LED_R, fm.fpioa.GPIO1, force = True)
fm.register(IO_LED_B, fm.fpioa.GPIO2, force = True)

# 定义板载LED GPIO的工作模式
LED_G=GPIO(GPIO.GPIO0, GPIO.OUT)
LED_R=GPIO(GPIO.GPIO1, GPIO.OUT)
LED_B=GPIO(GPIO.GPIO2, GPIO.OUT)

# 设置板载LED默认状态--熄灭
LED_G.value(1)
LED_R.value(1)
LED_B.value(1)

# 定义K210 IO输出引脚
# IO 6 Black Door Signal OUT
# IO 7 Blue  Door Signal OUT
# IO 8 Green Door Signal OUT

IO_BLACK_OUT = 6
IO_BLUE_OUT  = 7
IO_GREEN_OUT = 8

# 注册信号输出引脚到GPIO资源
fm.register(IO_BLACK_OUT, fm.fpioa.GPIO3, force = True)
fm.register(IO_BLUE_OUT,  fm.fpioa.GPIO4, force = True)
fm.register(IO_GREEN_OUT, fm.fpioa.GPIO5, force = True)

# 定义信号输出 GPIO的工作模式
BLACK_OUT = GPIO(GPIO.GPIO3, GPIO.OUT)
BLUE_OUT  = GPIO(GPIO.GPIO4, GPIO.OUT)
GREEN_OUT = GPIO(GPIO.GPIO5, GPIO.OUT)

# 设置信号输出的默认状态--不输出(0)
BLACK_OUT.value(0)
BLUE_OUT.value(0)
GREEN_OUT.value(0)

# 定义K210 使能EN输入引脚
# IO 9 STM32控制K210使能
IO_EN_IN = 9

# 注册使能EN引脚到GPIO资源
fm.register(IO_EN_IN, fm.fpioa.GPIO6, force = True)

# 定义使能EN GPIO的工作模式
EN_IN = GPIO(GPIO.GPIO6, GPIO.IN, GPIO.PULL_DOWN)

# 定义K210默认使能状态
K210_EN_State = False

# @name  BLACK_DOOR
# @brief 判断到黑色 K210板载红色LED亮起 排针IO6输出1
def BLACK_DOOR():
    # K210板载红色LED亮起
    LED_G.value(1)
    LED_R.value(0)
    LED_B.value(1)
    print("black")
    # 排针IO6输出1
    BLACK_OUT.value(1)
    BLUE_OUT.value(0)
    GREEN_OUT.value(0)

# @name  BLUE_DOOR
# @brief 判断到蓝色 K210板载蓝色LED亮起 排针IO7输出1
def BLUE_DOOR():
    # K210板载蓝色LED亮起
    LED_G.value(1)
    LED_R.value(1)
    LED_B.value(0)
    print("blue")
    # 排针IO7输出1
    BLACK_OUT.value(0)
    BLUE_OUT.value(1)
    GREEN_OUT.value(0)

# @name  GREEN_DOOR
# @brief 判断到绿色 K210板载绿色LED亮起 排针IO8输出1
def GREEN_DOOR():
    # K210板载绿色LED亮起
    LED_G.value(0)
    LED_R.value(1)
    LED_B.value(1)
    print("green")
    # 排针IO8输出1
    BLACK_OUT.value(0)
    BLUE_OUT.value(0)
    GREEN_OUT.value(1)

# @name  K210_EN_ON
# @brief 判断K210使能状态
def K210_EN_ON():
    global K210_EN_State
    utime.sleep_ms(100)
    if EN_IN.value() == 0:
        K210_EN_State = False
        print("K210 OFF")
    elif EN_IN.value() == 1:
        K210_EN_State = True
        print("K210 ON")
    return K210_EN_State

#################### 摄像头设置 ####################
# 重置并初始化感光元件 默认设置为 摄像头频率 24M 不开启双缓冲模式


sensor.reset()
# 设置图像格式为 RGB565 (彩色) 除此之外 还可设置格式为 GRAYSCALE 或者 YUV422
sensor.set_pixformat(sensor.RGB565)
# 设置图像大小为 QVGA (320 x 240) 像素个数 76800 K210最大支持格式为 VGA
sensor.set_framesize(sensor.QVGA)
# 设置自动曝光 默认
sensor.set_auto_exposure(1)
# 固定画面增益 每块K210都不同 需要调整
sensor.set_auto_gain(False, gain_db = 25)
# 固定画面白平衡 RGB增益 现在这块无法调节
sensor.set_auto_whitebal(0, rgb_gain_db = (0,0,0))
# 设置对比度 0 这个参数无法读取 且调这个参数对画面似乎不会产生影响 暂时注释
#sensor.set_contrast(0)
# 设置亮度 0 这个参数无法读取 且调这个参数对画面似乎不会产生影响 暂时注释
#sensor.set_brightness(0)
# 设置饱和度 0 这个参数无法读取 且调这个参数对画面似乎不会产生影响 暂时注释
#sensor.set_saturation(0)
# 打开垂直翻转
sensor.set_vflip(0)
# 打开水平镜像
sensor.set_hmirror(1)
# 延时跳过2s 等待感光元件稳定
sensor.skip_frames(time = 2000)
# 开始抓取图像
sensor.run(1)

clock = time.clock()
# lcd.init(freq=15000000)   //显示屏

#################### 颜色识别 ####################
# 颜色阈值
#thresholds_BBG = [(33, 69, 16, 55, 0, 56)   , # 黑色阈值
                  #(61, 89, -27, -4, 13, 58) , # 蓝色阈值
                  #(31, 62, -53, -27, 10, 48)] # 绿色阈值

#绿色阈值
green_threshold =(26,62,-5,-29,23,1)
#黑色阈值
black_threshold =(5,65,6,-5,7,-7)
#蓝色阈值
blue_threshold =(23,83,-11,27,-44,-6)
# 划定感兴趣区
color_roi = (100,0,180,240)


# 寻找最大色块
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    return max_blob


#################### 主程序循环 ####################
while True:
    clock.tick()
    img = sensor.snapshot()
    img.rotation_corr(z_rotation=90)  # 将图像逆时针旋转90度
    #lcd.display(img)
    img.draw_rectangle(color_roi)
    fps = clock.fps()
    img.draw_string(2,2, ("%2.1ffps" %(fps)), color=(0,128,0), scale=2)

    # blobs = img.find_blobs(thresholds,roi=color_roi)
    blobs = img.find_blobs([black_threshold,blue_threshold,green_threshold], roi = color_roi)

    if blobs:
        max_blob1 = find_max(blobs)
        if max_blob1.code() == 1:   # 黑色门
            img.draw_rectangle(max_blob1.rect())
            img.draw_cross(max_blob1.cx(), max_blob1.cy())
            BLACK_DOOR()
            img.draw_string(max_blob1.x(), max_blob1.y(), "BLACK", color = (0, 0, 0), scale = 2, mono_space = False)
        elif max_blob1.code() == 2: # 蓝色门
            img.draw_rectangle(max_blob1.rect())
            img.draw_cross(max_blob1.cx(), max_blob1.cy())
            BLUE_DOOR()
            img.draw_string(max_blob1.x(), max_blob1.y(), "BLUE ", color = (0, 0, 255), scale = 2, mono_space = False)
        elif max_blob1.code() == 4: # 绿色门
            img.draw_rectangle(max_blob1.rect())
            img.draw_cross(max_blob1.cx(), max_blob1.cy())
            GREEN_DOOR()
            img.draw_string(max_blob1.x(), max_blob1.y(), "GREEN", color = (0, 255, 0), scale = 2, mono_space = False)
